#include <algorithm>
#include <math.h>
#include <iostream>
#include "DrawingEngine.h"
#include "render/Objects/Renderer3DView.h"
#include "render/Objects/ImageRender.h"
#include <render/Objects/Renderer3DObject.h>
#include <render/Engine/DrawingPrimitives.h>
#include <render/Model/Model.h>
#include <render/Model/ModelH.h>

namespace pwp {

using namespace std;

DrawingEngine* DrawingEngine::instance;

DrawingEngine::DrawingEngine() { }
DrawingEngine::~DrawingEngine() { }

template <class T>
inline T min3(T t1, T t2, T t3)
{
  T minim; minim = (t1 < t2) ? t1 : t2; minim = (t3 < minim) ? t3 : minim; return minim;
}

template <class T>
inline T max3(T t1, T t2, T t3)
{
  T maxim; maxim = (t1 > t2) ? t1 : t2; maxim = (t3 > maxim) ? t3 : maxim; return maxim;
}

void DrawingEngine::drawFilled(ImageRender* imageRender, ModelH* drawingModel, int objectId)
{
  size_t i, j;

  VBYTE currentColor;

  //currentColor = 24 * (objectId + 1);
  currentColor = 200;

  for (j = 0; j < (*drawingModel->faces).size(); j++)
    this->drawFaceFilled(imageRender, (*drawingModel->faces)[j], drawingModel, currentColor);
}

void GetModelViewMatrix(const float* pose, float* returnMatrix) {
  returnMatrix[0] = pose[0];
  returnMatrix[1] = pose[4];
  returnMatrix[2] = pose[8];
  returnMatrix[3] = 0.0f;

  returnMatrix[4] = pose[1];
  returnMatrix[5] = pose[5];
  returnMatrix[6] = pose[9];
  returnMatrix[7] = 0.0f;

  returnMatrix[8] = pose[2];
  returnMatrix[9] = pose[6];
  returnMatrix[10] = pose[10];
  returnMatrix[11] = 0.0f;

  returnMatrix[12] = pose[3];
  returnMatrix[13] = pose[7];
  returnMatrix[14] = pose[11];
  returnMatrix[15] = 1.0f;
}

void DrawingEngine::ComputeAndSetPMMatrices(Renderer3DObject* object, Renderer3DView* renderView, const float* pose)
{
  float pmMatrix[16];
  float modelViewMatrixFull[16];

  GetModelViewMatrix(pose, modelViewMatrixFull);
  renderView->cameraCoordinateTransform->GetProjectionMatrix(projectionMatrix);

  MathUtils::SquareMatrixProduct(pmMatrix, projectionMatrix, modelViewMatrixFull, 4);
  memcpy(object->pmMatrix, pmMatrix, 16 * sizeof(float));
}

void DrawingEngine::Draw(
  Renderer3DObject* renderObject, Renderer3DView* view, const float* pose,
  ImageRender* imageRender, bool clearImage)
{
  Renderer3DView* renderView = view;

  ComputeAndSetPMMatrices(renderObject, view, pose);

  applyCoordinateTransform(renderView, renderObject, renderObject->pmMatrix);

  if (clearImage) {
    imageRender->Clear();
  }

  drawFilled(imageRender, renderObject->drawingModel, renderObject->objectId);
}

bool DrawingEngine::CalcBodyPoint(
  int ix, int iy, 
  Renderer3DView* view, ImageRender* render_images,
  float* x_unproj) 
{
  int width = render_images->width;
  int height = render_images->height;
  int icX = ix, icY = iy;
  int icZ = icX + icY * width;
  auto zbuffer = render_images->imageZBuffer;
  auto depth = zbuffer[icZ];

  if (MAX_INT == depth)
    return false;

  float xProjected[4];

  xProjected[0] = (float)2 * (icX - view->view[0]) / view->view[2] - 1.0f;
  xProjected[1] = (float)2 * (icY - view->view[1]) / view->view[3] - 1.0f;
  xProjected[2] = (float)2 * ((float)zbuffer[icZ] / (float)MAX_INT) - 1.0f;
  xProjected[3] = 1.0f;

  float xUnprojected[4];
  MathUtils::MatrixVectorProduct4(view->invP, xProjected, xUnprojected);

  x_unproj[0] = xUnprojected[0];
  x_unproj[1] = xUnprojected[1];
  x_unproj[2] = xUnprojected[2];
  return true;
}

void DrawingEngine::drawFaceEdges(ImageUChar* image, ModelFace* currentFace, ModelH* drawingModel, VBYTE color, int* extrems) {
  if (currentFace->verticesVector[0] * 4 > static_cast<int>(drawingModel->verticesVectorSize) * 4)
  {
    std::cerr << "Fatal Error! overflow! verticesVectorSize is " << drawingModel->verticesVectorSize <<
      ", desire pos is " << currentFace->verticesVector[0] * 4 + 0 << std::endl;
    return;
  }

  VFLOAT x1 = drawingModel->verticesVector[currentFace->verticesVector[0] * 4 + 0];
  VFLOAT y1 = drawingModel->verticesVector[currentFace->verticesVector[0] * 4 + 1];

  if (isfinite(x1) == false)
  {
    printf("  [drawFaceEdges] fatal error! invalid value of x.\n");
    exit(-1);
  }

  VFLOAT x2 = drawingModel->verticesVector[currentFace->verticesVector[1] * 4 + 0];
  VFLOAT y2 = drawingModel->verticesVector[currentFace->verticesVector[1] * 4 + 1];

  VFLOAT x3 = drawingModel->verticesVector[currentFace->verticesVector[2] * 4 + 0];
  VFLOAT y3 = drawingModel->verticesVector[currentFace->verticesVector[2] * 4 + 1];

  DRAWLINE(image, x1, y1, x2, y2, color);
  DRAWLINE(image, x2, y2, x3, y3, color);
  DRAWLINE(image, x1, y1, x3, y3, color);

  extrems[0] = (VINT)x1;
  extrems[1] = (VINT)y1;
  extrems[2] = (VINT)x1;
  extrems[3] = (VINT)y1;

  extrems[0] = (VINT)MIN(extrems[0], x2);
  extrems[1] = (VINT)MIN(extrems[1], y2);
  extrems[2] = (VINT)MAX(extrems[2], x2);
  extrems[3] = (VINT)MAX(extrems[3], y2);

  extrems[0] = (VINT)MIN(extrems[0], x3);
  extrems[1] = (VINT)MIN(extrems[1], y3);
  extrems[2] = (VINT)MAX(extrems[2], x3);
  extrems[3] = (VINT)MAX(extrems[3], y3);

  //  printf("extrems (%d,%d,%d,%d)\n",extrems[0], extrems[1], extrems[2], extrems[3] );
  //  std::cout<<"finish draw face "<<std::endl;
}

void DrawingEngine::drawFaceFilled(ImageRender* imageRender, ModelFace& currentFace, ModelH* drawingModel, VBYTE color)
{
  if (currentFace.verticesVector[0] * 4 > static_cast<int>(drawingModel->verticesVectorSize) * 4)
  {
    std::cerr << "Fatal Error! overflow! verticesVectorSize is " << drawingModel->verticesVectorSize <<
      ", desire pos is " << currentFace.verticesVector[0] * 4 + 0 << std::endl;
    return;
  }

  VFLOAT x1 = drawingModel->verticesVector[currentFace.verticesVector[0] * 4 + 0];
  VFLOAT y1 = drawingModel->verticesVector[currentFace.verticesVector[0] * 4 + 1];
  VFLOAT z1 = drawingModel->verticesVector[currentFace.verticesVector[0] * 4 + 2];

  VFLOAT x2 = drawingModel->verticesVector[currentFace.verticesVector[1] * 4 + 0];
  VFLOAT y2 = drawingModel->verticesVector[currentFace.verticesVector[1] * 4 + 1];
  VFLOAT z2 = drawingModel->verticesVector[currentFace.verticesVector[1] * 4 + 2];

  VFLOAT x3 = drawingModel->verticesVector[currentFace.verticesVector[2] * 4 + 0];
  VFLOAT y3 = drawingModel->verticesVector[currentFace.verticesVector[2] * 4 + 1];
  VFLOAT z3 = drawingModel->verticesVector[currentFace.verticesVector[2] * 4 + 2];

  size_t i;
  size_t index;
  VUINT intZ;
  VFLOAT dx1, dx2, dx3, dz1, dz2, dz3, dxa, dxb, dza, dzb;
  VFLOAT dzX, Sz, Sx, Sy, Ex;

  VECTOR3DA S, E;
  VECTOR3DA A, B, C;
  VECTOR3DA orderedPoints[3];

  A = VECTOR3DA(x1, y1, z1);
  B = VECTOR3DA(x2, y2, z2);
  C = VECTOR3DA(x3, y3, z3);

  if (y1 < y2)
  {
    if (y3 < y1) { orderedPoints[0] = C; orderedPoints[1] = A; orderedPoints[2] = B; }
    else if (y3 < y2) { orderedPoints[0] = A; orderedPoints[1] = C; orderedPoints[2] = B; }
    else { orderedPoints[0] = A; orderedPoints[1] = B; orderedPoints[2] = C; }
  }
  else
  {
    if (y3 < y2) { orderedPoints[0] = C; orderedPoints[1] = B; orderedPoints[2] = A; }
    else if (y3 < y1) { orderedPoints[0] = B; orderedPoints[1] = C; orderedPoints[2] = A; }
    else { orderedPoints[0] = B; orderedPoints[1] = A;	orderedPoints[2] = C; }
  }

  A = orderedPoints[0]; B = orderedPoints[1]; C = orderedPoints[2];

  dx1 = (B.y - A.y) > 0 ? (B.x - A.x) / (B.y - A.y) : B.x - A.x;
  dx2 = (C.y - A.y) > 0 ? (C.x - A.x) / (C.y - A.y) : 0;
  dx3 = (C.y - B.y) > 0 ? (C.x - B.x) / (C.y - B.y) : 0;

  dz1 = (B.y - A.y) != 0 ? (B.z - A.z) / (B.y - A.y) : 0;
  dz2 = (C.y - A.y) != 0 ? (C.z - A.z) / (C.y - A.y) : 0;
  dz3 = (C.y - B.y) != 0 ? (C.z - B.z) / (C.y - B.y) : 0;

  S = E = A;

  B.y = floor(B.y - 0.5f); C.y = floor(C.y - 0.5f);

  if (dx1 > dx2) { dxa = dx2; dxb = dx1; dza = dz2; dzb = dz1; }
  else { dxa = dx1; dxb = dx2; dza = dz1; dzb = dz2; }

  //for (; S.y <= B.y; S.y++, E.y++, S.x += dxa, E.x += dxb, S.z += dza, E.z += dzb)
  while(S.y <= B.y)
  {
    dzX = (E.x != S.x) ? (E.z - S.z) / (E.x - S.x) : 0;
    Sz = S.z;

    Sy = CLAMP(S.y, 0, (VFLOAT)imageRender->height - 1);
    Sx = CLAMP(S.x, 0, (VFLOAT)imageRender->width - 1);
    Ex = CLAMP(E.x, 0, (VFLOAT)imageRender->width - 1);

    for (i = (size_t)Sx; i < Ex; i++)
    {
      index = PIXELMATINDEX(i, Sy, imageRender->width);
      intZ = (unsigned int)(MAX_INT * Sz);

      if (intZ < imageRender->imageZBuffer[index])
      {
        imageRender->imageFill[index] = color;
        imageRender->imageZBuffer[index] = intZ;
      }
      Sz += dzX;
    }
    S.y++, E.y++;
    S.x += dxa, E.x += dxb;
    S.z += dza, E.z += dzb;
  }

  if (dx1 > dx2) { dxa = dx2; dxb = dx3; dza = dz2; dzb = dz3; E = B; }
  else { dxa = dx3; dxb = dx2; dza = dz3; dzb = dz2; S = B; }

  //for (; S.y <= C.y; S.y++, E.y++, S.x += dxa, E.x += dxb, S.z += dza, E.z += dzb)
  while(S.y <= C.y)
  {
    dzX = (E.x != S.x) ? (E.z - S.z) / (E.x - S.x) : 0;
    Sz = S.z;

    Sy = CLAMP(S.y, 0, (VFLOAT)imageRender->height - 1);
    Sx = CLAMP(S.x, 0, (VFLOAT)imageRender->width - 1);
    Ex = CLAMP(E.x, 0, (VFLOAT)imageRender->width - 1);

    for (i = (size_t)Sx; i < Ex; i++)
    {
      index = PIXELMATINDEX(i, Sy, imageRender->width);
      intZ = (unsigned int)(MAX_INT * Sz);

      if (intZ < imageRender->imageZBuffer[index])
      {
        imageRender->imageFill[index] = color;
        imageRender->imageZBuffer[index] = intZ;
      }
      Sz += dzX;
    }

    S.y++, E.y++;
    S.x += dxa, E.x += dxb;
    S.z += dza, E.z += dzb;
  }
}

void DrawingEngine::applyCoordinateTransform(Renderer3DView* view, Renderer3DObject* object, float* pmMatrix)
{
  size_t i;

  object->model->ToModelH(object->drawingModel);


  for (i = 0; i < object->drawingModel->verticesVectorSize; i++)
  {
    VFLOAT* originalVertexAsDouble = &object->drawingModel->originalVerticesVector[i * 4];
    VFLOAT* vertexAsDouble = &object->drawingModel->verticesVector[i * 4];

    MathUtils::MatrixVectorProduct4(pmMatrix, originalVertexAsDouble, vertexAsDouble);

    vertexAsDouble[0] = view->view[0] + view->view [2] * (vertexAsDouble[0] + 1.f) / 2.f;
    vertexAsDouble[1] = view->view[1] + view->view[3] * (vertexAsDouble[1] + 1.f) / 2.f;
    vertexAsDouble[2] = (vertexAsDouble[2] + 1) / 2;
  }
}

}